/********************************************************************************
MIT License

Copyright(c) 3dRudder 2017

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files(the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and / or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions :

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
********************************************************************************/
#include "stdafx.h"
#include "ServerDriver.h"
#include "ServerDriverHost.h"
#include "DriverContext.h"
#include <process.h>
#include "..\Common\SharedData.h"
#include "..\Common\MathTools.h"

float GetAxisValue(ns3dRudder::Axis *pAxis, CSISharedData::AxisValue nType)
{
	float fRet = 0;
	switch (nType)
	{
	case CSISharedData::avDefault:
		break;
	case CSISharedData::avLeftRight:
		fRet = pAxis->m_aX;
		break;
	case CSISharedData::avBackwardForward:
		fRet = pAxis->m_aY;
		break;
	case CSISharedData::avUpDown:
		fRet = pAxis->m_aZ;
		break;
	case CSISharedData::avRotationLeftRight:
		fRet = pAxis->m_rZ;
		break;
	default:
		break;
	}
	return fRet;
}

float GetHalfAxisValue(ns3dRudder::Axis *pAxis, CSISharedData::HalfAxisValue nType)
{
	float fRet = 0;
	switch (nType)
	{
	case CSISharedData::havDefault:
		break;
	case CSISharedData::havRight:
		fRet = clamp(pAxis->m_aX, 0, 1);
		break;
	case CSISharedData::havLeft:
		fRet = abs(clamp(pAxis->m_aX, -1, 0));
		break;
	case CSISharedData::havForward:
		fRet = clamp(pAxis->m_aY, 0, 1);
		break;
	case CSISharedData::havBackward:
		fRet = abs(clamp(pAxis->m_aY, -1, 0));
		break;
	case CSISharedData::havUp:
		fRet = clamp(pAxis->m_aZ, 0, 1);
		break;
	case CSISharedData::havDown:
		fRet = abs(clamp(pAxis->m_aZ, -1, 0));
		break;
	case CSISharedData::havRotationRight:
		fRet = clamp(pAxis->m_rZ, 0, 1);
		break;
	case CSISharedData::havRotationLeft:
		fRet = abs(clamp(pAxis->m_rZ, -1, 0));
		break;
	default:
		break;
	}
	return fRet;
}

BOOL g_bThreadGoOut = FALSE;


void OnMemoryMapManager(void *_pDataTh)
{
	CSharedMemory SharedMemory;
	CSharedMemory SharedMemoryDebug;

	if (SharedMemoryDebug.Open(_T("3dR_SI_DATA_DEBUG"), sizeof(CSISharedDataDebug)))
	{
		DEBUG_LOG_SETCOLOR(DEBUG_LOG_RED, DEBUG_LOG_BLACK);
		DEBUG_LOG(1, "Fail to open memory map debug");
		DEBUG_LOG_SETCOLOR(DEBUG_LOG_WHITE, DEBUG_LOG_BLACK);
	}

	if (!SharedMemory.Open(_T("3dR_SI_DATA"), sizeof(CSISharedData)))
	{
		DEBUG_LOG_SETCOLOR(DEBUG_LOG_RED, DEBUG_LOG_BLACK);
		DEBUG_LOG(1, "Fail to open memory map");
		DEBUG_LOG_SETCOLOR(DEBUG_LOG_WHITE, DEBUG_LOG_BLACK);
	}

	while (!g_bThreadGoOut)
	{
		CVRServerDriverHost *pDriverHost = (CVRServerDriverHost*)vr::VRServerDriverHost();

		CDriverInstance *pDriverInstance = pDriverHost->m_pDriverInstance;

		SharedMemory.Read((uint8_t*)pDriverInstance->m_pSharedData, sizeof(CSISharedData));
		SharedMemoryDebug.Write((uint8_t*)pDriverInstance->m_pSharedDataDebug, sizeof(CSISharedDataDebug));

		CSI_Message *pMessage;
		while (pMessage = pDriverInstance->m_pSharedMessageHandleIn->PopMessage())
		{
			CMovement *pMovement = pDriverInstance->m_pMovement;
			switch (pMessage->m_nType)
			{
			case CSiMessageeType::Reset:
			{
				CSI_MessageReset *pMessageReset = (CSI_MessageReset*)pMessage;
				if (pMessageReset->m_bReset)
					pMovement->Reset();
			}
			break;
			case CSiMessageeType::Calibration:
			{
				CSI_MessageCalibration *pMessageCalib = (CSI_MessageCalibration*)pMessage;
				if (pMessageCalib->m_bCalib)
				{
					pMovement->m_bInCalibration = true;
					pMovement->Reset(true);
				}
				else
				{
					memcpy(&pMovement->m_mMatrix34Hmd, &pMessageCalib->m_matrix, sizeof(CSISharedData::Matrix34));
					pMovement->m_bAskCalibration = true;
					pMovement->m_bInCalibration = false;
				}
			}
			default:
				break;
			}

			delete pMessage;
		}
		Sleep(200);
	}
	_endthread();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// CServerDriver
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

EVRInitError CServerDriver::Init(IVRDriverContext *pDriverContext)
{
	EVRInitError nError = vr::VRInitError_Init_NotInitialized;

	m_pDriverContext = new CVRDriverContext(pDriverContext, m_pDriverInstance);
	VR_INIT_SERVER_DRIVER_CONTEXT(m_pDriverContext);

	if (m_pDriverInstance->m_pChildServerTrackedDeviceProvider)
		nError = m_pDriverInstance->m_pChildServerTrackedDeviceProvider->Init(m_pDriverContext);
	m_TimeFrame.start();
	DEBUG_LOG_SETCOLOR(DEBUG_LOG_WHITE, DEBUG_LOG_BLACK);
	DEBUG_LOG(1, "CServerDriver::Init()=>%d", nError);

	_beginthread(OnMemoryMapManager, 0, nullptr);

	return nError;
}

/** cleans up the driver right before it is unloaded */
void CServerDriver::Cleanup()
{
	g_bThreadGoOut = TRUE;
	DEBUG_LOG_SETCOLOR(DEBUG_LOG_WHITE, DEBUG_LOG_BLACK);
	DEBUG_LOG(1, "CServerDriver::Cleanup()");

	if (m_pDriverInstance->m_pChildServerTrackedDeviceProvider)
		m_pDriverInstance->m_pChildServerTrackedDeviceProvider->Cleanup();

	if (m_pDriverContext)
		delete m_pDriverContext;
	m_pDriverContext = NULL;
}

/** Returns the version of the ITrackedDeviceServerDriver interface used by this driver */
const char * const *CServerDriver::GetInterfaceVersions()
{
	const char *const *pRet = NULL;
	if (m_pDriverInstance->m_pChildServerTrackedDeviceProvider)
		pRet = m_pDriverInstance->m_pChildServerTrackedDeviceProvider->GetInterfaceVersions();

#if DEBUG_LOG_LEVEL >0
	char sText[2048];
	memset(sText, 0, 2048);
	int i = 0;
	if (pRet)
		while (pRet[i])
		{
			strcat(sText, pRet[i]);
			strcat(sText, ",");
			i++;
		}

	DEBUG_LOG_SETCOLOR(DEBUG_LOG_BLUE, DEBUG_LOG_BLACK);
	DEBUG_LOG(1, "CServerDriver::GetInterfaceVersions()=>");
	DEBUG_LOG(1, sText);
#endif

	return pRet;
}

void ButtonPressed(CVRServerDriverHost *pDriverHost, CTrackedDeviceServerDriver *pTrackedDevice, bool pressed)
{
	if (pressed)
		pDriverHost->TrackedDeviceButtonPressed(pTrackedDevice->m_nID, EVRButtonId::k_EButton_Axis0, 0);
	else
		pDriverHost->TrackedDeviceButtonUnpressed(pTrackedDevice->m_nID, EVRButtonId::k_EButton_Axis0, 0);
	pTrackedDevice->m_bPressed = pressed;
	pTrackedDevice->m_bUnPressed = !pressed;
}

void ButtonTouched(CVRServerDriverHost *pDriverHost, CTrackedDeviceServerDriver *pTrackedDevice, bool pressed)
{
	if (pressed)
		pDriverHost->TrackedDeviceButtonTouched(pTrackedDevice->m_nID, EVRButtonId::k_EButton_Axis0, 0);
	else
		pDriverHost->TrackedDeviceButtonUntouched(pTrackedDevice->m_nID, EVRButtonId::k_EButton_Axis0, 0);
	pTrackedDevice->m_bTouched = pressed;
	pTrackedDevice->m_bUnTouched = !pressed;
}

/** Allows the driver do to some work in the main loop of the server. */
void CServerDriver::RunFrame()
{
	if (m_pDriverInstance->m_pChildServerTrackedDeviceProvider)
		m_pDriverInstance->m_pChildServerTrackedDeviceProvider->RunFrame();

	ns3dRudder::Axis OurAxis;
	ns3dRudder::Status status;

	ns3dRudder::CSdk *pSdk = m_pDriverInstance->m_pSdk;
	CMovement *pMovement = m_pDriverInstance->m_pMovement;
	CSISharedData *pSharedData = m_pDriverInstance->m_pSharedData;

	if (pSdk)
	{
		// Create curves
		ns3dRudder::CurveArray CurveArray;
		ns3dRudder::Curve Curve[4];

		// Get shared curves
		Curve[ns3dRudder::CurveXAxis] = pSharedData->m_Curve[ns3dRudder::CurveXAxis];
		Curve[ns3dRudder::CurveYAxis] = pSharedData->m_Curve[ns3dRudder::CurveYAxis];
		Curve[ns3dRudder::CurveZAxis] = pSharedData->m_Curve[ns3dRudder::CurveZAxis];
		Curve[ns3dRudder::CurveZRotation] = pSharedData->m_Curve[ns3dRudder::CurveZRotation];

		// Set curves
		CurveArray.SetCurve(ns3dRudder::CurveXAxis, &Curve[ns3dRudder::CurveXAxis]);
		CurveArray.SetCurve(ns3dRudder::CurveYAxis, &Curve[ns3dRudder::CurveYAxis]);
		CurveArray.SetCurve(ns3dRudder::CurveZAxis, &Curve[ns3dRudder::CurveZAxis]);
		CurveArray.SetCurve(ns3dRudder::CurveZRotation, &Curve[ns3dRudder::CurveZRotation]);

		// Get Axis (Symmetrical Pitch || Curves)
		if (pSharedData->m_bNonSymPitch)
			pSdk->GetAxis(0, ns3dRudder::ValueWithCurveNonSymmetricalPitch, &OurAxis, &CurveArray);
		else
			pSdk->GetAxis(0, ns3dRudder::ValueWithCurve, &OurAxis, &CurveArray);
		
		// Get Status
		status = pSdk->GetStatus(0);
		if (status >= ns3dRudder::InUse)
		{
			if (!m_bHoldInUse)
			{
				m_bHoldInUse = true;
				if (pSharedData->m_bCalibrateOnInUse)
				{
					pMovement->m_bInCalibration = true;
					pMovement->Reset(true);
				}
			}
		}
		else
		{
			m_bHoldInUse = false;
		}
	}

	CVRServerDriverHost *pDriverHost = (CVRServerDriverHost*)vr::VRServerDriverHost();
	CSISharedDataDebug *pSharedDataDebug = m_pDriverInstance->m_pSharedDataDebug;

	// Delta Time
	float fDeltaTime = float(m_TimeFrame.stop() / 1000000.0) / 1000.0f;
	pSharedDataDebug->m_fDeltaTime = fDeltaTime;

	m_TimeFrame.start();
	if (!pMovement->m_bInCalibration && pDriverHost->IsCalibrated())
	{
		if (pSharedData->m_bActivateVRLocomotion)
		{
			// Speed Axis
			float fSpeedAx = pSharedData->m_fSpeed[0] * fDeltaTime;
			float fSpeedAy = pSharedData->m_fSpeed[1] * fDeltaTime;
			float fSpeedAz = pSharedData->m_fSpeed[2] * fDeltaTime;
			float fSpeedRz = pSharedData->m_fSpeed[3] * fDeltaTime;

			// update translation
			HmdVector3d_t trans;
			if (pSharedData->m_bSmoothMode)
			{
				trans.v[0] = pMovement->m_SmoothRoll.ComputeSpeed(OurAxis.m_aX, fSpeedAx, pSharedData->m_fSmoothValue[0], fDeltaTime); // Roll (X)			
				trans.v[1] = pMovement->m_SmoothUpDown.ComputeSpeed(-OurAxis.m_aZ, fSpeedAz, pSharedData->m_fSmoothValue[2], fDeltaTime); // Updown (Y)			
				trans.v[2] = pMovement->m_SmoothPitch.ComputeSpeed(OurAxis.m_aY, fSpeedAy, pSharedData->m_fSmoothValue[1], fDeltaTime); // Pitch (Z)																															 
				pMovement->m_fAngle += pMovement->m_SmoothYaw.ComputeSpeed(OurAxis.m_rZ, fSpeedRz, pSharedData->m_fSmoothValue[3], fDeltaTime); // Yaw (rZ)
			}
			else
			{
				trans.v[0] = OurAxis.m_aX * fSpeedAx; // Roll (X)			
				trans.v[1] = -OurAxis.m_aZ * fSpeedAz; // Updown (Y)			
				trans.v[2] = OurAxis.m_aY * fSpeedAy; // Pitch (Z)													  
				pMovement->m_fAngle += OurAxis.m_rZ * fSpeedRz; // Yaw (rZ)
			}
			// Transform the movement's vector with initial hmd rotation and 3dRudder rotation
			Transform(pMovement->m_qRotationInitial * pMovement->m_qRotation, trans);
			for (int i = 0; i < 3; ++i)
				pMovement->m_v3Translation.v[i] += trans.v[i];

			// Update HMD & Controllers
			DriverServerMap::iterator it = pDriverHost->m_MapDeviceServerDriver.begin();
			while (it != pDriverHost->m_MapDeviceServerDriver.end())
			{
				CTrackedDeviceServerDriver* pTrackedDevice = (it->second);
				// Only controllers
				if (pTrackedDevice->m_DeviceClass != ETrackedDeviceClass::TrackedDeviceClass_Invalid && 
					pTrackedDevice->m_DeviceClass != ETrackedDeviceClass::TrackedDeviceClass_TrackingReference)
				{
					pDriverHost->TrackedDevicePoseUpdated(pTrackedDevice->m_nID, pTrackedDevice->m_currentPose, sizeof(DriverPose_t));
				}
				it++;
			}
		}

		// Update offset Y
		pMovement->m_v3OffsetY = MultVector(pMovement->m_v3AxisY, -pSharedData->m_fHeightOffset);

		/******DEBUG******/
		pSharedDataDebug->m_TranslationXYZAngleW.m_fx = pMovement->m_v3Translation.v[0];
		pSharedDataDebug->m_TranslationXYZAngleW.m_fy = pMovement->m_v3Translation.v[1];
		pSharedDataDebug->m_TranslationXYZAngleW.m_fz = pMovement->m_v3Translation.v[2];
		pSharedDataDebug->m_TranslationXYZAngleW.m_fw = pMovement->m_fAngle;

		pSharedDataDebug->m_SmoothSpeed.m_fx = pMovement->m_SmoothRoll.speedCurrent;
		pSharedDataDebug->m_SmoothSpeed.m_fy = pMovement->m_SmoothUpDown.speedCurrent;
		pSharedDataDebug->m_SmoothSpeed.m_fz = pMovement->m_SmoothPitch.speedCurrent;
		pSharedDataDebug->m_SmoothSpeed.m_fw = pMovement->m_SmoothYaw.speedCurrent;
		/******END DEBUG******/
	}

	if (pSharedData->m_bActivateCtrls)
	{
		DriverServerMap::iterator it = pDriverHost->m_MapDeviceServerDriver.begin();
		while (it != pDriverHost->m_MapDeviceServerDriver.end())
		{
			CTrackedDeviceServerDriver* pTrackedDevice = (it->second);
			ETrackedPropertyError Error = ETrackedPropertyError::TrackedProp_Success;

			if (pTrackedDevice->m_nID != k_ulOverlayHandleInvalid && pTrackedDevice->m_DeviceClass == ETrackedDeviceClass::TrackedDeviceClass_Controller)
			{
				int nDataIndex = -1;
				if (pSharedData->m_nPadIndex[0] == pTrackedDevice->m_nID)  // Left controller
					nDataIndex = 0;
				if (pSharedData->m_nPadIndex[1] == pTrackedDevice->m_nID) // Right controller
					nDataIndex = 1;

				if (nDataIndex >= 0)
				{
					if (pSdk)
					{
						if (status >= ns3dRudder::InUse)
						{
							VRControllerAxis_t MyAxisState;
							bool bPressed = false;
							int indexPad = nDataIndex * 2;
							if (pSharedData->m_nAxis[0 + indexPad] != CSISharedData::avDefault || pSharedData->m_nAxis[1 + indexPad] != CSISharedData::avDefault)
							{
								MyAxisState.x = GetAxisValue(&OurAxis, pSharedData->m_nAxis[0 + indexPad]);
								MyAxisState.y = GetAxisValue(&OurAxis, pSharedData->m_nAxis[1 + indexPad]);

								if (fabs(MyAxisState.x) > 0.1 || fabs(MyAxisState.y) > 0.1)
									bPressed = true;

								if (bPressed)
									if (!pTrackedDevice->m_bPressed)
										ButtonPressed(pDriverHost, pTrackedDevice, true);
								else
									if (!pTrackedDevice->m_bUnPressed)
										ButtonPressed(pDriverHost, pTrackedDevice, false);

								// update axis trackpad
								pDriverHost->TrackedDeviceAxisUpdated(pTrackedDevice->m_nID, 0, MyAxisState);
							}
							// update axis trigger
							if (pSharedData->m_nHalfAxis[nDataIndex] != CSISharedData::havDefault)
							{
								MyAxisState.x = GetHalfAxisValue(&OurAxis, pSharedData->m_nHalfAxis[nDataIndex]);
								MyAxisState.y = 0.0f;
								pDriverHost->TrackedDeviceAxisUpdated(pTrackedDevice->m_nID, 1, MyAxisState);
							}

							if (!pTrackedDevice->m_bTouched)
								ButtonTouched(pDriverHost, pTrackedDevice, true);
						}
						else
						{
							if (!pTrackedDevice->m_bUnPressed)
								ButtonPressed(pDriverHost, pTrackedDevice, false);
							if (!pTrackedDevice->m_bUnTouched)
								ButtonTouched(pDriverHost, pTrackedDevice, false);
						}
					}
				}
			}
			it++;
		}
	}
	DEBUG_LOG_SETCOLOR(DEBUG_LOG_WHITE, DEBUG_LOG_BLACK);
	DEBUG_LOG(5, "CServerDriver::RunFrame()");
}

// ------------  Power State Functions ----------------------- //

/** Returns true if the driver wants to block Standby mode. */
bool CServerDriver::ShouldBlockStandbyMode()
{
	bool bRet = false;
	if (m_pDriverInstance->m_pChildServerTrackedDeviceProvider)
		bRet = m_pDriverInstance->m_pChildServerTrackedDeviceProvider->ShouldBlockStandbyMode();

	DEBUG_LOG_SETCOLOR(DEBUG_LOG_WHITE, DEBUG_LOG_BLACK);
	DEBUG_LOG(1, "CServerDriver::RunFrame()=>%d", bRet);

	return false;
}


/** Called when the system is entering Standby mode. The driver should switch itself into whatever sort of low-power
* state it has. */
void CServerDriver::EnterStandby()
{
	if (m_pDriverInstance->m_pChildServerTrackedDeviceProvider)
		m_pDriverInstance->m_pChildServerTrackedDeviceProvider->EnterStandby();

	DEBUG_LOG_SETCOLOR(DEBUG_LOG_WHITE, DEBUG_LOG_BLACK);
	DEBUG_LOG(1, "CServerDriver::EnterStandby()");
}


/** Called when the system is leaving Standby mode. The driver should switch itself back to
full operation. */
void CServerDriver::LeaveStandby()
{
	if (m_pDriverInstance->m_pChildServerTrackedDeviceProvider)
		m_pDriverInstance->m_pChildServerTrackedDeviceProvider->LeaveStandby();
	DEBUG_LOG_SETCOLOR(DEBUG_LOG_WHITE, DEBUG_LOG_BLACK);
	DEBUG_LOG(1, "CServerDriver::LeaveStandby()");
}
